#include "board.hpp"
#include <iostream>
#include "board.hpp"

#include "io_context/common.hpp"
using drivers::common::IoContext;
// board.cpp

Board::Board(const std::string &device_port, int baudrate, int timeout_ms)
{
    drivers::serial_driver::SerialPortConfig config(
    baudrate,
    drivers::serial_driver::FlowControl::NONE,
    drivers::serial_driver::Parity::NONE,
    drivers::serial_driver::StopBits::ONE);

    auto io_context_ = std::make_shared<drivers::common::IoContext>(1);
    // 初始化 serial_driver_
    serial_driver_ = std::make_shared<drivers::serial_driver::SerialDriver>(*io_context_);
    serial_driver_->init_port(device_port, config);
    serial_driver_->port()->open();

    auto port = serial_driver_->port();

}



Board::~Board()
{
    serial_driver_->port()->close();
}

void Board::enable_reception()
{
    enable_recv = true;
    std::thread(&Board::recv_task, this).detach();
}

void Board::packet_report_sys(const std::vector<uint8_t> &data)
{
    if (sys_queue.size() >= 1)
    {
        sys_queue.pop(); // 清除旧数据，只保留最新的一帧
    }
    sys_queue.push(data);
}

void Board::packet_report_key(const std::vector<uint8_t> &data)
{
    if (key_queue.size() >= 1)
    {
        key_queue.pop();
    }
    key_queue.push(data);
}

void Board::packet_report_imu(const std::vector<uint8_t> &data)
{
    if (imu_queue.size() >= 1)
    {
        imu_queue.pop();
    }
    imu_queue.push(data);
}

void Board::packet_report_gamepad(const std::vector<uint8_t> &data)
{
    if (gamepad_queue.size() >= 1)
    {
        gamepad_queue.pop();
    }
    gamepad_queue.push(data);
}

void Board::packet_report_serial_servo(const std::vector<uint8_t> &data)
{
    if (bus_servo_queue.size() >= 1)
    {
        bus_servo_queue.pop();
    }
    bus_servo_queue.push(data);
}

void Board::packet_report_pwm_servo(const std::vector<uint8_t> &data)
{
    if (pwm_servo_queue.size() >= 1)
    {
        pwm_servo_queue.pop();
    }
    pwm_servo_queue.push(data);
}

void Board::packet_report_sbus(const std::vector<uint8_t> &data)
{
    if (sbus_queue.size() >= 1)
    {
        sbus_queue.pop();
    }
    sbus_queue.push(data);
}

void Board::buf_write(PacketFunction func, const std::vector<uint8_t> &data)
{
    std::vector<uint8_t> buf = {0xAA, 0x55, static_cast<uint8_t>(func)};
    buf.push_back(static_cast<uint8_t>(data.size()));
    buf.insert(buf.end(), data.begin(), data.end());
    buf.push_back(checksum_crc8(std::vector<uint8_t>(buf.begin() + 2, buf.end())));
    this->serial_driver_->port()->send(buf);
}

uint16_t Board::get_battery()
{
    if (enable_recv)
    {
        if (!sys_queue.empty())
        {
            std::vector<uint8_t> data = sys_queue.front();
            sys_queue.pop();
            if (data[0] == 0x04)
            {
                return static_cast<uint16_t>(data[1]) | (static_cast<uint16_t>(data[2]) << 8);
            }
        }
    }
    else
    {
        std::cout << "enable reception first!" << std::endl;
    }
    return 0;
}

void Board::enable_reception()
{
    enable_recv = true;
    std::thread(&Board::recv_task, this).detach();
}

std::pair<uint8_t, uint8_t> Board::get_button()
{
    if (enable_recv)
    {
        if (!key_queue.empty())
        {
            std::vector<uint8_t> data = key_queue.front();
            key_queue.pop();
            uint8_t key_id = data[0];
            PacketReportKeyEvents key_event = static_cast<PacketReportKeyEvents>(data[1]);
            if (key_event == PacketReportKeyEvents::KEY_EVENT_CLICK)
            {
                return {key_id, 0};
            }
            else if (key_event == PacketReportKeyEvents::KEY_EVENT_PRESSED)
            {
                return {key_id, 1};
            }
        }
    }
    else
    {
        std::cout << "enable reception first!" << std::endl;
    }
    return {0, 0};
}

std::vector<float> Board::get_imu()
{
    if (enable_recv)
    {
        if (!imu_queue.empty())
        {
            std::vector<uint8_t> data = imu_queue.front();
            imu_queue.pop();
            std::vector<float> result(6);
            std::memcpy(result.data(), data.data(), 6 * sizeof(float));
            return result;
        }
    }
    else
    {
        std::cout << "enable reception first!" << std::endl;
    }
    return {};
}

std::pair<std::vector<float>, std::vector<int>> Board::get_gamepad()
{
    if (enable_recv)
    {
        if (!gamepad_queue.empty())
        {
            std::vector<uint8_t> data = gamepad_queue.front();
            gamepad_queue.pop();
            uint16_t buttons;
            uint8_t hat;
            int8_t lx, ly, rx, ry;
            std::memcpy(&buttons, data.data(), sizeof(uint16_t));
            std::memcpy(&hat, data.data() + sizeof(uint16_t), sizeof(uint8_t));
            std::memcpy(&lx, data.data() + sizeof(uint16_t) + sizeof(uint8_t), sizeof(int8_t));
            std::memcpy(&ly, data.data() + sizeof(uint16_t) + sizeof(uint8_t) + sizeof(int8_t), sizeof(int8_t));
            std::memcpy(&rx, data.data() + sizeof(uint16_t) + sizeof(uint8_t) + 2 * sizeof(int8_t), sizeof(int8_t));
            std::memcpy(&ry, data.data() + sizeof(uint16_t) + sizeof(uint8_t) + 3 * sizeof(int8_t), sizeof(int8_t));

            std::vector<float> axes(8, 0.0);
            std::vector<int> buttons_vec(16, 0);

            // 处理按钮和轴数据
            // ...

            return {axes, buttons_vec};
        }
    }
    else
    {
        std::cout << "enable reception first!" << std::endl;
    }
    return {{}, {}};
}

std::vector<float> Board::get_sbus()
{
    if (enable_recv)
    {
        if (!sbus_queue.empty())
        {
            std::vector<uint8_t> data = sbus_queue.front();
            sbus_queue.pop();
            SBusStatus status;
            std::memcpy(status.channels.data(), data.data(), 16 * sizeof(int16_t));
            status.channel_17 = data[16 * sizeof(int16_t)] != 0;
            status.channel_18 = data[16 * sizeof(int16_t) + 1] != 0;
            status.signal_loss = data[16 * sizeof(int16_t) + 2] != 0;
            status.fail_safe = data[16 * sizeof(int16_t) + 3] != 0;

            std::vector<float> result;
            if (status.signal_loss)
            {
                result = std::vector<float>(16, 0.5);
                result[4] = 0;
                result[5] = 0;
                result[6] = 0;
                result[7] = 0;
            }
            else
            {
                for (int16_t i : status.channels)
                {
                    result.push_back(2 * (i - 192) / (1792 - 192) - 1);
                }
            }
            return result;
        }
    }
    else
    {
        std::cout << "enable reception first!" << std::endl;
    }
    return {};
}

void Board::set_led(float on_time, float off_time, uint8_t repeat = 1, uint8_t led_id = 1)
{
    uint16_t on_time_ms = static_cast<uint16_t>(on_time * 1000);
    uint16_t off_time_ms = static_cast<uint16_t>(off_time * 1000);
    std::vector<uint8_t> data;
    data.push_back(led_id);
    data.push_back(on_time_ms & 0xFF);
    data.push_back((on_time_ms >> 8) & 0xFF);
    data.push_back(off_time_ms & 0xFF);
    data.push_back((off_time_ms >> 8) & 0xFF);
    data.push_back(repeat & 0xFF);
    data.push_back((repeat >> 8) & 0xFF);
    buf_write(PacketFunction::PACKET_FUNC_LED, data);
}

void Board::set_buzzer(uint16_t freq, float on_time, float off_time, uint8_t repeat = 1)
{
    uint16_t on_time_ms = static_cast<uint16_t>(on_time * 1000);
    uint16_t off_time_ms = static_cast<uint16_t>(off_time * 1000);
    std::vector<uint8_t> data;
    data.push_back(freq & 0xFF);
    data.push_back((freq >> 8) & 0xFF);
    data.push_back(on_time_ms & 0xFF);
    data.push_back((on_time_ms >> 8) & 0xFF);
    data.push_back(off_time_ms & 0xFF);
    data.push_back((off_time_ms >> 8) & 0xFF);
    data.push_back(repeat & 0xFF);
    data.push_back((repeat >> 8) & 0xFF);
    buf_write(PacketFunction::PACKET_FUNC_BUZZER, data);
}

void Board::set_motor_speed(const std::vector<std::pair<uint8_t, float>> &speeds)
{
    std::vector<uint8_t> data = {0x01, static_cast<uint8_t>(speeds.size())};
    for (const auto &speed : speeds)
    {
        data.push_back(speed.first - 1);
        uint32_t float_bits;
        std::memcpy(&float_bits, &speed.second, sizeof(float));
        data.push_back(float_bits & 0xFF);
        data.push_back((float_bits >> 8) & 0xFF);
        data.push_back((float_bits >> 16) & 0xFF);
        data.push_back((float_bits >> 24) & 0xFF);
    }
    buf_write(PacketFunction::PACKET_FUNC_MOTOR, data);
}

void Board::pwm_servo_set_position(float duration, const std::vector<std::pair<uint8_t, uint16_t>> &positions)
{
    uint16_t duration_ms = static_cast<uint16_t>(duration * 1000);
    std::vector<uint8_t> data = {0x01, duration_ms & 0xFF, (duration_ms >> 8) & 0xFF, static_cast<uint8_t>(positions.size())};
    for (const auto &position : positions)
    {
        data.push_back(position.first);
        data.push_back(position.second & 0xFF);
        data.push_back((position.second >> 8) & 0xFF);
    }
    buf_write(PacketFunction::PACKET_FUNC_PWM_SERVO, data);
}

void Board::pwm_servo_set_offset(uint8_t servo_id, int8_t offset)
{
    std::vector<uint8_t> data = {0x07, servo_id, static_cast<uint8_t>(offset)};
    buf_write(PacketFunction::PACKET_FUNC_PWM_SERVO, data);
}

int8_t Board::pwm_servo_read_offset(uint8_t servo_id)
{
    std::lock_guard<std::mutex> lock(pwm_servo_read_lock);
    std::vector<uint8_t> data = {0x09, servo_id};
    buf_write(PacketFunction::PACKET_FUNC_PWM_SERVO, data);
    std::this_thread::sleep_for(std::chrono::seconds(1));
    if (!pwm_servo_queue.empty())
    {
        std::vector<uint8_t> response = pwm_servo_queue.front();
        pwm_servo_queue.pop();
        return static_cast<int8_t>(response[2]);
    }
    return 0;
}

uint16_t Board::pwm_servo_read_position(uint8_t servo_id)
{
    std::lock_guard<std::mutex> lock(pwm_servo_read_lock);
    std::vector<uint8_t> data = {0x05, servo_id};
    buf_write(PacketFunction::PACKET_FUNC_PWM_SERVO, data);
    std::this_thread::sleep_for(std::chrono::seconds(1));
    if (!pwm_servo_queue.empty())
    {
        std::vector<uint8_t> response = pwm_servo_queue.front();
        pwm_servo_queue.pop();
        return static_cast<uint16_t>(response[2]) | (static_cast<uint16_t>(response[3]) << 8);
    }
    return 0;
}

void Board::bus_servo_enable_torque(uint8_t servo_id, bool enable)
{
    std::vector<uint8_t> data;
    if (enable)
    {
        data = {0x0B, servo_id};
    }
    else
    {
        data = {0x0C, servo_id};
    }
    buf_write(PacketFunction::PACKET_FUNC_BUS_SERVO, data);
    std::this_thread::sleep_for(std::chrono::milliseconds(20));
}

void Board::bus_servo_set_id(uint8_t servo_id_now, uint8_t servo_id_new)
{
    std::vector<uint8_t> data = {0x10, servo_id_now, servo_id_new};
    buf_write(PacketFunction::PACKET_FUNC_BUS_SERVO, data);
    std::this_thread::sleep_for(std::chrono::milliseconds(20));
}

void Board::bus_servo_set_offset(uint8_t servo_id, int8_t offset)
{
    std::vector<uint8_t> data = {0x20, servo_id, static_cast<uint8_t>(offset)};
    buf_write(PacketFunction::PACKET_FUNC_BUS_SERVO, data);
    std::this_thread::sleep_for(std::chrono::milliseconds(20));
}

void Board::bus_servo_save_offset(uint8_t servo_id)
{
    std::vector<uint8_t> data = {0x24, servo_id};
    buf_write(PacketFunction::PACKET_FUNC_BUS_SERVO, data);
    std::this_thread::sleep_for(std::chrono::milliseconds(20));
}

void Board::bus_servo_set_angle_limit(uint8_t servo_id, const std::pair<uint16_t, uint16_t> &limit)
{
    std::vector<uint8_t> data = {0x30, servo_id};
    data.push_back(limit.first & 0xFF);
    data.push_back((limit.first >> 8) & 0xFF);
    data.push_back(limit.second & 0xFF);
    data.push_back((limit.second >> 8) & 0xFF);
    buf_write(PacketFunction::PACKET_FUNC_BUS_SERVO, data);
    std::this_thread::sleep_for(std::chrono::milliseconds(20));
}

void Board::bus_servo_set_vin_limit(uint8_t servo_id, const std::pair<uint16_t, uint16_t> &limit)
{
    std::vector<uint8_t> data = {0x34, servo_id};
    data.push_back(limit.first & 0xFF);
    data.push_back((limit.first >> 8) & 0xFF);
    data.push_back(limit.second & 0xFF);
    data.push_back((limit.second >> 8) & 0xFF);
    buf_write(PacketFunction::PACKET_FUNC_BUS_SERVO, data);
    std::this_thread::sleep_for(std::chrono::milliseconds(20));
}

void Board::bus_servo_set_temp_limit(uint8_t servo_id, int8_t limit)
{
    std::vector<uint8_t> data = {0x38, servo_id, static_cast<uint8_t>(limit)};
    buf_write(PacketFunction::PACKET_FUNC_BUS_SERVO, data);
    std::this_thread::sleep_for(std::chrono::milliseconds(20));
}

void Board::bus_servo_stop(const std::vector<uint8_t> &servo_id)
{
    std::vector<uint8_t> data = {0x03, static_cast<uint8_t>(servo_id.size())};
    data.insert(data.end(), servo_id.begin(), servo_id.end());
    buf_write(PacketFunction::PACKET_FUNC_BUS_SERVO, data);
}

void Board::bus_servo_set_position(float duration, const std::vector<std::pair<uint8_t, uint16_t>> &positions)
{
    uint16_t duration_ms = static_cast<uint16_t>(duration * 1000);
    std::vector<uint8_t> data = {0x01, duration_ms & 0xFF, (duration_ms >> 8) & 0xFF, static_cast<uint8_t>(positions.size())};
    for (const auto &position : positions)
    {
        data.push_back(position.first);
        data.push_back(position.second & 0xFF);
        data.push_back((position.second >> 8) & 0xFF);
    }
    buf_write(PacketFunction::PACKET_FUNC_BUS_SERVO, data);
}

std::vector<int> Board::bus_servo_read_and_unpack(uint8_t servo_id, uint8_t cmd, const std::string &unpack)
{
    std::lock_guard<std::mutex> lock(servo_read_lock);
    std::vector<uint8_t> data = {cmd, servo_id};
    buf_write(PacketFunction::PACKET_FUNC_BUS_SERVO, data);
    std::this_thread::sleep_for(std::chrono::seconds(1));
    if (!bus_servo_queue.empty())
    {
        std::vector<uint8_t> response = bus_servo_queue.front();
        bus_servo_queue.pop();
        std::vector<int> result;
        // 解析响应数据
        // ...
        return result;
    }
    return {};
}

int Board::bus_servo_read_id(uint8_t servo_id = 254)
{
    std::vector<int> result = bus_servo_read_and_unpack(servo_id, 0x12, "<BBbB");
    if (!result.empty())
    {
        return result[0];
    }
    return -1;
}

int Board::bus_servo_read_offset(uint8_t servo_id)
{
    std::vector<int> result = bus_servo_read_and_unpack(servo_id, 0x22, "<BBbb");
    if (!result.empty())
    {
        return result[0];
    }
    return 0;
}

int Board::bus_servo_read_position(uint8_t servo_id)
{
    std::vector<int> result = bus_servo_read_and_unpack(servo_id, 0x05, "<BBbh");
    if (!result.empty())
    {
        return result[0];
    }
    return 0;
}

int Board::bus_servo_read_vin(uint8_t servo_id)
{
    std::vector<int> result = bus_servo_read_and_unpack(servo_id, 0x07, "<BBbH");
    if (!result.empty())
    {
        return result[0];
    }
    return 0;
}

int Board::bus_servo_read_temp(uint8_t servo_id)
{
    std::vector<int> result = bus_servo_read_and_unpack(servo_id, 0x09, "<BBbB");
    if (!result.empty())
    {
        return result[0];
    }
    return 0;
}

int Board::bus_servo_read_temp_limit(uint8_t servo_id)
{
    std::vector<int> result = bus_servo_read_and_unpack(servo_id, 0x3A, "<BBbB");
    if (!result.empty())
    {
        return result[0];
    }
    return 0;
}

std::pair<int, int> Board::bus_servo_read_angle_limit(uint8_t servo_id)
{
    std::vector<int> result = bus_servo_read_and_unpack(servo_id, 0x32, "<BBb2H");
    if (result.size() >= 2)
    {
        return {result[0], result[1]};
    }
    return {0, 0};
}

std::pair<int, int> Board::bus_servo_read_vin_limit(uint8_t servo_id)
{
    std::vector<int> result = bus_servo_read_and_unpack(servo_id, 0x36, "<BBb2H");
    if (result.size() >= 2)
    {
        return {result[0], result[1]};
    }
    return {0, 0};
}

int Board::bus_servo_read_torque_state(uint8_t servo_id)
{
    std::vector<int> result = bus_servo_read_and_unpack(servo_id, 0x0D, "<BBbb");
    if (!result.empty())
    {
        return result[0];
    }
    return 0;
}

void Board::recv_task()
{
    while (Board::enable_recv)
    {
        uint8_t recv_data;
        serial_driver_->read(&recv_data, 1);
        switch (state)
        {
        case PacketControllerState::PACKET_CONTROLLER_STATE_STARTBYTE1:
            if (recv_data == 0xAA)
            {
                state = PacketControllerState::PACKET_CONTROLLER_STATE_STARTBYTE2;
            }
            break;
        case PacketControllerState::PACKET_CONTROLLER_STATE_STARTBYTE2:
            if (recv_data == 0x55)
            {
                state = PacketControllerState::PACKET_CONTROLLER_STATE_FUNCTION;
            }
            else
            {
                state = PacketControllerState::PACKET_CONTROLLER_STATE_STARTBYTE1;
            }
            break;
        case PacketControllerState::PACKET_CONTROLLER_STATE_FUNCTION:
            if (recv_data < static_cast<uint8_t>(PacketFunction::PACKET_FUNC_NONE))
            {
                frame = {recv_data, 0};
                state = PacketControllerState::PACKET_CONTROLLER_STATE_LENGTH;
            }
            else
            {
                frame.clear();
                state = PacketControllerState::PACKET_CONTROLLER_STATE_STARTBYTE1;
            }
            break;
        case PacketControllerState::PACKET_CONTROLLER_STATE_LENGTH:
            frame[1] = recv_data;
            recv_count = 0;
            if (recv_data == 0)
            {
                state = PacketControllerState::PACKET_CONTROLLER_STATE_CHECKSUM;
            }
            else
            {
                state = PacketControllerState::PACKET_CONTROLLER_STATE_DATA;
            }
            break;
        case PacketControllerState::PACKET_CONTROLLER_STATE_DATA:
            frame.push_back(recv_data);
            recv_count++;
            if (recv_count >= frame[1])
            {
                state = PacketControllerState::PACKET_CONTROLLER_STATE_CHECKSUM;
            }
            break;
        case PacketControllerState::PACKET_CONTROLLER_STATE_CHECKSUM:
        {
            uint8_t crc8 = checksum_crc8(frame);
            if (crc8 == recv_data)
            {
                PacketFunction func = static_cast<PacketFunction>(frame[0]);
                std::vector<uint8_t> data(frame.begin() + 2, frame.end());
                switch (func)
                {
                case PacketFunction::PACKET_FUNC_SYS:
                    packet_report_sys(data);
                    break;
                case PacketFunction::PACKET_FUNC_KEY:
                    packet_report_key(data);
                    break;
                case PacketFunction::PACKET_FUNC_IMU:
                    packet_report_imu(data);
                    break;
                case PacketFunction::PACKET_FUNC_GAMEPAD:
                    packet_report_gamepad(data);
                    break;
                case PacketFunction::PACKET_FUNC_BUS_SERVO:
                    packet_report_serial_servo(data);
                    break;
                case PacketFunction::PACKET_FUNC_PWM_SERVO:
                    packet_report_pwm_servo(data);
                    break;
                case PacketFunction::PACKET_FUNC_SBUS:
                    packet_report_sbus(data);
                    break;
                default:
                    break;
                }
            }
            else
            {
                std::cout << "校验失败" << std::endl;
            }
            state = PacketControllerState::PACKET_CONTROLLER_STATE_STARTBYTE1;
            break;
        }
        }
    }
    std::cout << "END..." << std::endl;
}